#include <iostream>
#include "TcpServer.h"

using namespace std;
using namespace BORNS;

int main()
{
    TcpServer server{8888};
    server.SetRcvFunc([&](std::vector<uint8_t> &bytes, const TcpProtocol::Device &dev){
        static auto timer = std::chrono::high_resolution_clock::now();
        std::string str1 = std::string(reinterpret_cast<const char*>(bytes.data()), bytes.size());
//        cout<< "recv: "<< str1<<endl;
        std::string str2{""};
        int endIndex = -1;
        for(unsigned long i = 0; i < str1.size(); ++i){
            if(str1[i] == ';'){
                endIndex = i;
                str2 = str1.substr(0, i);
                bytes.erase(bytes.cbegin(), bytes.cbegin() + i + 1);
                break;
            }
        }
        std::vector<std::string> strVec = TcpProtocol::split(str2, ',');
        if(std::stoi(strVec[0]) == 1){// 命令类型为1表示发的位姿值
            std::vector<double> pose;
            for(unsigned long i = 1; i < strVec.size(); ++i){
                pose.push_back(std::stod(strVec[i]));
            }
//            cout<<"recv pose:";
//            for(const auto &ele:pose){
//                cout<<" "<<ele;
//            }
//            cout<<endl;
        }
        cout<<"cycle time="<<std::chrono::duration_cast<std::chrono::microseconds>(
                  std::chrono::high_resolution_clock::now() - timer).count()<<endl;
        timer = std::chrono::high_resolution_clock::now();
//        cout<<"remain bytes size: "<<bytes.size()<<endl;
    });
    while(1){
        usleep(100000);
    }
    return 0;
}
